#include "J3DModel.h"
#include "JGLWidget.h"
#include "JLog.h"
#include "JProject.h"

#include <QFile>
#include <QFileDialog>
#include <QStringList>
#include <math.h>

#define PI 3.141592653589793
#define R2D (180.0/PI)

//static void Inches_to_Millimeters(STLModel& model)
//{
//	for(unsigned int i=0; i<model.numFaces(); ++i)
//	{
//		for(unsigned int j = 0; j<3; ++j)
//		{
//			model._faces[i]->_centroid[j] = model._faces[i]->_centroid[j]*25.4;
//			model._faces[i]->_v1[j] = model._faces[i]->_v1[j]*25.4;
//			model._faces[i]->_v2[j] = model._faces[i]->_v2[j]*25.4;
//			model._faces[i]->_v3[j] = model._faces[i]->_v3[j]*25.4;
//			//model._faces[i]->_normal[j] = model._faces[i]->_normal[j];			
//		}
//	}
//}

static void toQuaternion312(double z_rot, double x_rot, double y_rot, double* rotation)
{
	//UNIT: degree to radian
	float rr[3];    //Rotation in Radians
	rr[0] = z_rot /R2D;  // z
	rr[1] = x_rot /R2D;  // x
	rr[2] = y_rot /R2D;  // y

	//UNIT: radian
	//Rotation in Radians

	// suppose the rotation matrix is R.
	// The quaternion generated from this approach always has a non-negative
	//  scalar part (rotation[0]).
	float c1 = cos(rr[0]);
	float s1 = sin(rr[0]);
	float c2 = cos(rr[1]);
	float s2 = sin(rr[1]);
	float c3 = cos(rr[2]);
	float s3 = sin(rr[2]);
	float R11 = c1*c3-s1*s2*s3;
	float R22 = c1*c2;
	float R33 = c2*c3;
	float trace = R11+R22+R33;   // = 4coshalf^2-1 = 2cosine+1
	if (trace<-1) trace = -1;

	// avoid certain compiler optimization error and/or potential rounding error,
	// see 'case 312' in GetPose() for more info.
	// Actual bug case encountered here.
	volatile float coshalf_tmp = sqrt((trace+1)/4); // >=0. scalar part of the quaternion.
	float coshalf = coshalf_tmp;
	// 'volatile' to avoid potential compiler optimization error,
	// see 'case 312' in GetPose() for more info.
	float R12 = -s1*c2;
	float R13 = c1*s3+s1*s2*c3;
	float R21 = s1*c3+c1*s2*s3;
	float R23 = s1*s3-c1*s2*c3;
	float R31 = -c2*s3;
	float R32 = s2;
	rotation[0] = coshalf;  // my q0
	if (coshalf==0)  //180deg rotation
	{
		// For 180deg rotation, two opposite axes are possible. We make the
		// 1st element of the vector part always positive (unless it's zero,
		// in which case we will make the 2nd element positive, or the 3rd).

		volatile float tmp = sqrt( R22+R33>0 ? 0 : -(R22+R33)/2 );
		// 'volatile' to avoid potential compiler optimization error,
		// see 'case 312' in GetPose() for more info.
		// '?:' to avoid potential compiler optimization error and/or rounding error.
		rotation[1] = tmp;
		if (rotation[1]==0)
		{
			tmp = sqrt( R11+R33>0 ? 0 : -(R11+R33)/2 );
			// '?:' to avoid potential compiler optimization error and/or rounding error.
			rotation[2] = tmp;
			if (rotation[2]==0)
				rotation[3] = sqrt( R11+R22>0 ? 0 : -(R11+R22)/2 );
			// '?:' to avoid potential compiler optimization error and/or rounding error.
			else
				rotation[3] = R32/rotation[2]/2;
		}
		else
		{
			rotation[2] = R21/rotation[1]/2;
			rotation[3] = R31/rotation[1]/2;
		}
	}
	else
	{
		rotation[1] = (R32-R23)/4/coshalf;
		rotation[2] = (R13-R31)/4/coshalf;
		rotation[3] = (R21-R12)/4/coshalf;
	}
}


J3DModel::J3DModel(JGLWidget* glwidget, const QString& filename)
	: m_Model(filename),
	m_bShow(true),
	m_bLoop(true),
	nbKeyFrames(10)
{
	//Inches_to_Millimeters(m_Model);
	//Log.Post("Inches to Millimeters");

	m_ModelDisplayList = glGenLists(1);
	glNewList(m_ModelDisplayList,GL_COMPILE);
	glPushMatrix();
	glBegin(GL_TRIANGLES);
	for (unsigned int i = 0; i < m_Model.numFaces(); i++)
	{
		glNormal3f(m_Model._faces[i]->_normal[0], m_Model._faces[i]->_normal[1], m_Model._faces[i]->_normal[2]);
		glVertex3f(m_Model._faces[i]->_v1[0], m_Model._faces[i]->_v1[1], m_Model._faces[i]->_v1[2]);
		glVertex3f(m_Model._faces[i]->_v2[0], m_Model._faces[i]->_v2[1], m_Model._faces[i]->_v2[2]);
		glVertex3f(m_Model._faces[i]->_v3[0], m_Model._faces[i]->_v3[1], m_Model._faces[i]->_v3[2]);
	}
	glEnd();
	glPopMatrix();
	glEndList();
	Log.Post("STL Model Loaded: " + filename, JLOG_SUCCESS);

	connect(glwidget, SIGNAL(drawNeeded()), this, SLOT(draw()));
	//glwidget->updateGL();

	// myFrame is the Frame that will be interpolated.
	qglviewer::Frame* myFrame = new qglviewer::Frame();

	// Set myFrame as the KeyFrameInterpolator interpolated Frame.
	kfi_.setFrame(myFrame);
	kfi_.setLoopInterpolation(m_bLoop);

	// JTS file loading
	QString filePath = QFileDialog::getOpenFileName(NULL, 
		tr("Select a JointTrack Kinematics File to Load"),
		Project.FilePath(), tr("JTS files (*.jts)"));

	QFile file(filePath);
	/*if ( file.open( QIODevice::ReadOnly | QIODevice::Text ) )
	{
		int i = 0;
		while(!file.atEnd()){
			file.readLine();
			i++;
		}
		nbKeyFrames = i;
	}
	Log.Post(QString::number(nbKeyFrames));
	file.close();*/

	// An array of manipulated (key) frames.
	keyFrame_ = new qglviewer::ManipulatedFrame*[nbKeyFrames];

	Project.AddKinematics(filePath);

	// Create an initial path	
	if ( file.open( QIODevice::ReadOnly | QIODevice::Text ) ) 
	{
		int nRow = 0;
		while ( !file.atEnd() ) 
		{
			QByteArray line = file.readLine();
			QStringList vals = QString(line).split(','/*QRegExp(" *, *")*/, QString::SkipEmptyParts);

			if (nRow >= 2)
			{
				keyFrame_[nRow-2] = new qglviewer::ManipulatedFrame();

				keyFrame_[nRow-2]->setPosition(vals[0].toFloat(), vals[1].toFloat(), vals[2].toFloat()+140);

 				double rotation[4];
 				toQuaternion312(vals[3].toDouble(),vals[4].toDouble(),vals[5].toDouble(), rotation);
 				/*Log.PostSub( QString::number(rotation[0])+" "+
 					QString::number(rotation[1])+" "+
 					QString::number(rotation[2])+" "+
 					QString::number(rotation[3]));*/
 				keyFrame_[nRow-2]->setRotation(rotation[1],rotation[2],rotation[3],rotation[0]);
				
				kfi_.addKeyFrame(keyFrame_[nRow-2]);

				Log.PostSub( QString::number(nRow-1) + 
					"_ Trans: ("+ QString::number(vals[0].toFloat())+ ", "+ 
					QString::number(vals[1].toFloat())+ ", "+
					QString::number(vals[2].toFloat())+
					") Rot312: ("+ QString::number(vals[3].toFloat())+ ", "+
					QString::number(vals[4].toFloat())+ ", "+ 
					QString::number(vals[5].toFloat())+ ")" );
			}
			nRow++;
		}
		Log.Post("JTS File Loaded: "+file.fileName(), JLOG_SUCCESS);
	}
	file.close();

	/*for (int i=0; i<nbKeyFrames; i++)
	{
		keyFrame_[i] = new qglviewer::ManipulatedFrame();
		keyFrame_[i]->setPosition(-1.0 + 5.0*i/(nbKeyFrames-1), 2.0*i, 2.0*i);
		kfi_.addKeyFrame(keyFrame_[i]);
	}*/

	currentKF_ = 0;
	glwidget->setManipulatedFrame(keyFrame_[currentKF_]);

	connect(&kfi_, SIGNAL(interpolated()), glwidget, SLOT(updateGL()));
	//kfi_.startInterpolation();
	glwidget->updateGL();

}

J3DModel::~J3DModel()
{

}

void J3DModel::draw()
{
	
	// Draw interpolated frame
	glPushMatrix();
	glMultMatrixd(kfi_.frame()->matrix());
	//drawAxis(0.3f);
	if(m_bShow)
		glCallList(m_ModelDisplayList);
	glPopMatrix();

	//kfi_.drawPath(5, 10);

	//for (int i=0; i<nbKeyFrames; ++i)
	//{
	//	glPushMatrix();
	//	glMultMatrixd(kfi_.keyFrame(i).matrix());

	//	//if ((i == currentKF_) || (keyFrame_[i]->grabsMouse()))
	//	//	;//drawAxis(0.4f);
	//	//else
	//	//	;//drawAxis(0.2f);

	//	glPopMatrix();
	//}

}

void J3DModel::play()
{
	kfi_.startInterpolation();
}

void J3DModel::reset()
{
	kfi_.resetInterpolation();
}

void J3DModel::stop()
{
	kfi_.resetInterpolation();
}

void J3DModel::pause()
{
	kfi_.stopInterpolation();
}

void J3DModel::loop()
{
	if (m_bLoop)
		m_bLoop = false;
	else
		m_bLoop = true;
		
	kfi_.setLoopInterpolation(m_bLoop);
}